#include "drop_cutter.hpp"
#include "boxtests.hpp"
#include <iostream>
#include <boost/numeric/ublas/io.hpp>

#include <map>
using namespace std;


/////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////


spacetree::spacetree():tree(2)
{
}
  


spacetree::spacetree(const triar &model,int cells):tree(cells)
{
   
  double xmin,ymin,zmin=100000,
    xmax,ymax,zmax=-100000;
    
  for(triar::const_iterator tri=model.begin();
      tri!=model.end();
      ++tri)
    for(triang::const_iterator p=tri->begin();
	p!=tri->end();
	++p)
      {
	if(p->x<xmin)
	  xmin=p->x;
	if(p->x>xmax)
	  xmax=p->x;
	if(p->y<ymin)
	  ymin=p->y;
	if(p->y>ymax)
	  ymax=p->y;
	if(p->z<zmin)
	  zmin=p->z;
	if(p->z>zmax)
	  zmax=p->z;
      }
  box=bbox(xmin,ymin,zmin,
	   xmax-xmin,ymax-ymin,zmax-zmin);

   double ps=model.size();
 double pc=0;
 int perc=0;

  for(triar::const_iterator p=model.begin();
      p!=model.end();
      ++p)
    {
      sort_traverse(box,*p);

      if((++pc/ps*100.0)>perc+1)
	{perc+=1;  cerr<<"\rclustering triangles: "<<perc;}
      
    }
  cerr<<"--\n";
  
}

void spacetree::sort_traverse(const bbox& b,const triang &tri)
{ 
  
  if(tri_box_int(b,tri))
    {
      if(b.depth<tree.size())
	{
	   
	  for(int i=0; i<8; i++)
	  sort_traverse(b.subbox(i),tri);
	}
      
      else
	tree(b.ix,b.iy,b.iz).push_back(tri);
    }
}

void spacetree::cyltraverse(const matr &frame,double d,triar *tris)
  
{
  point o;
  point n;
    
  o.x=frame(0,3);
  o.y=frame(1,3);
  o.z=frame(2,3);

  n.x=frame(0,2);
  n.y=frame(1,2);
  n.z=frame(2,2);

  n.normalize();
   
  cyltraverse(o,n, d,box,tris);
    
}
void spacetree::cyltraverse(const point &o, const point &n,double d,triar *tris)
  
{
   
  cyltraverse(o,n, d,box,tris);
    
}  
void  spacetree::cyltraverse(const point &o, const point &n,double d,
			     const bbox &b, triar *tris)
{
 
  if(ray_box_int(b.extend(d/2),o,n))
    {
      if(b.depth<tree.size())
	for(int i=0; i<8; i++)
	  cyltraverse(o,n,d,b.subbox(i),tris);
      else
	{
	  triar &ar=tree(b.ix,b.iy,b.iz);
	  
	  
	  for(triar::const_iterator p=ar.begin();
	      p!=ar.end();
	      ++p)
	    tris->push_back(*p);
	}
    }
}
  
/////////////////////////////////////////////////////////////

drop_cylinder_cutter_3d::drop_cylinder_cutter_3d(const triar &model,
						 int cells):tree(model,cells)
  {
  }

point drop_cylinder_cutter_3d::spec_tri_normal(double x,double y, double *z)
{
  point o(x,y,0);
  point d(0,0,1);
  triar tris;
  tree.cyltraverse(o,d,0,&tris);

  //cerr<<"-->tris.size "<<tris.size()<<endl;
  
  
  double z_dummy;
  if(z==NULL)
    z=&z_dummy;
  
  
  double tmax=-1000;

  triar::const_iterator tr;
    
  for(triar::const_iterator p=tris.begin();
      p!=tris.end();
      ++p)
    {
      double t;
      bool hit=ray_tri_int(*p,o,d,&t);
      if(hit)
	if(t>tmax)
	  {
	    tmax=t;
	    tr=p;
	  }
    }
  if(tmax==-1000)
    {
      *z=0;
      return point(0,0,1);
    }
  
  else
    {
      *z=tmax;
      return tr->normal();
    }
  
  
}

point drop_cylinder_cutter_3d::gen_pathpoint(double tool_diameter,double safe_z,
					       const point &o, const point &d)
{
  
  matr ori=fit_orient(o,d);
  //FIXIT
  triar tris;
  tree.cyltraverse(ori,tool_diameter,&tris);

  //cerr<<"****"<<tris.size();
  vector <double> zz;

  if(tris.size()!=0)
    for(triar::const_iterator p=tris.begin();
	p!=tris.end();
	++p)
      {
	double z;
	if(intersect_triangle(*p,ori,tool_diameter/2,&z))
	  {
	    zz.push_back(z);
	  }
	
      }
  matr shift=identity();

  if(zz.size()==0)
    shift(2,3)=safe_z;
  else
    shift(2,3)=max(zz);
  
  return T2pnt(prod(ori,shift));
  
}
point drop_cylinder_cutter_3d::gen_5axis_pathpoint(double tool_diameter,double safe_z,
			  const point &o, const point &ref_dir)
{
  double x=o.x,
    y=o.y,
    z;

  point dir=spec_tri_normal(x,y,&z);

   point p(x,y,z);

   point p1=gen_pathpoint(tool_diameter,safe_z,p,dir);
  point p2=gen_pathpoint(tool_diameter,safe_z,o,ref_dir);

 

  point d1=p1-p;
  point d2=p2-p;

  // cerr<<"*"<< x<<" "<<y<<" " << z <<" - "<<d1.l()<<"  "<<d2.l()<<endl;
  

  if(d2.l2()<d1.l2())
    return p2;
  else
    return p1;
    
}


vector<point>  drop_cylinder_cutter_3d::gen_toolpath(double tool_diameter,double dx,double dy,double safe_z,
						     tracing_note *note)
{
  tracing_note dummy;
  if(note==NULL)
    note=&dummy;
  
  
  toolpath.clear();
  
  double xmin=tree.xmin(),
    xmax=tree.xmax(),
    ymin=tree.ymin(),
    ymax=tree.ymax();
  
  
  
  double ps=(xmax-xmin)*(ymax-ymin)/dx/dy;
  double pc=0;
  int perc=0;

  bool forward_move=true;
  
  for(double x=xmin; x<=xmax; x+=dx)
    if(forward_move)
      {
	for(double y=ymin;
	    y<ymax;
	    y+=dy)
	  {
	    toolpath.push_back(gen_5axis_pathpoint(tool_diameter, safe_z,
					   point(x,y,0),point(0,0,1)));

	    if((++pc/ps*100.0)>perc+1){perc+=1;  note->tic("tracing",perc,*this); }
	  }
  	toolpath.push_back(gen_5axis_pathpoint(tool_diameter, safe_z,
				       point(x,ymax,0),point(0,0,1)));
	forward_move=!forward_move;
      }
    else
      {
	for(double y=ymax;
	    y>ymin;
	    y-=dy)
	  {
	    toolpath.push_back(gen_5axis_pathpoint(tool_diameter, safe_z,
					   point(x,y,0),point(0,0,1)));

	    if((++pc/ps*100.0)>perc+1)
	      {perc+=1;  note->tic("tracing",perc,*this);}
	  }
  	toolpath.push_back(gen_5axis_pathpoint(tool_diameter, safe_z,
					 point(x,ymin,0),point(0,0,1)));
	forward_move=!forward_move;
      }
  
  return toolpath;
  
}

/////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////

bool in_tri(const triang &tri,
		   double x, double y)
{
  return in_tri(tri[0],tri[1],tri[2],x,y);
}


int reject=0;
int accept=0;



bool drop_cylinder_cutter_2d::drop_cutter(const cluster &clus,
					  double x,double y,double r,double safe_z,double *z)
{
 
  if(x+r < clus.xmin ||
     x-r > clus.xmax ||
     y+r < clus.ymin ||
     y-r > clus.ymax )
    return false;
  
  vector<double> zz;
  
  if(clus.clus.size()!=0)
    for(vector<cluster>::const_iterator pc=clus.clus.begin();
	pc!=clus.clus.end();
	++pc)
      {
	double z;
	if(drop_cutter(*pc,x,y,r,safe_z,&z))
	{
	  zz.push_back(z);
	  accept++;
	}
	else
	  reject++;
	
	
      }
  else
    for(triar::const_iterator tri=clus.tris.begin();
	tri!=clus.tris.end();
	++tri)
      {
	double z=0;
	
	if(intersect_triangle(*tri,x,y,r,&z))
	  zz.push_back(z);
      }
  
  if(zz.size()==0)
    return false;
  else
    *z=max(zz);
  return true;
  
  
}


void drop_cylinder_cutter_2d::setmodel(const triar &_model)
{
	clus.tris=_model;
  xmin=xmax=clus.xmin=clus.xmax=clus.tris[0][0].x;
  ymax=ymin=clus.ymin=clus.ymax=clus.tris[0][0].y;
  clus.clus.clear();
  
  for( triar::const_iterator tri=clus.tris.begin();
       tri!=clus.tris.end();
       tri++)
    for(triang::const_iterator p=tri->begin();
	p!=tri->end();
	p++)
      {
	if(p->x<xmin)
	  xmin=p->x;
	if(p->x>xmax)
	  xmax=p->x;
	if(p->y<ymin)
	  ymin=p->y;
	if(p->y>ymax)
	  ymax=p->y;
      }
  clus.ymin=ymin;
  clus.ymax=ymax;
  clus.xmin=xmin;
  clus.xmax=xmax;
  
}




vector<point> drop_cylinder_cutter_2d::gen_toolpath(double tool_diameter,double dx,double dy,double safe_z)
{
  toolpath.clear();
  

  double r=tool_diameter/2;
  
        
     
  //set cluster size;
  if(cx==NA || cy==NA)
    {
      c0x=(xmax-xmin)/4;
      c0y=(ymax-ymin)/4;
      c1x=c0x/4;
      c1y=c0y/4;
      cx=c1x/4;
      cy=c1y/4;
    }
  
      

  //clustering
  if(clus.clus.empty()){
	down_sort(4,4,clus);	
	cerr<<"clust0\n"<<endl; 
	down_sort(4,4,clus);
   cerr<<"clust1\n"<<endl;
  down_sort(4,4,clus);
 //  cerr<<"clust2\n"<<endl;
   //down_sort(4,4,clus);  
   cerr<<"clustered.\n"<<endl;
  }
  //building trajectory

  cerr<<endl;
  
  
 double ps=(xmax-xmin)*(ymax-ymin)/dx/dy;
 double pc=0;
 int perc=0;

  bool forward_move=true;

   for(double x=xmin; x<=xmax; x+=dx)       
    if(forward_move)
      {
	for(double y=ymin; y<ymax; y+=dy)
	  {
	    double z;
	    if(drop_cutter(clus,x,y,r,safe_z,&z))
	      toolpath.push_back(point(x,y,z));
	    else
	      toolpath.push_back(point(x,y,safe_z));
	    
	    if((++pc/ps*100.0)>perc+1)
	      {	perc+=1;  cerr<<"\rtracing: "<<perc<<"% rejected:"<<reject<<" accepted:"<<accept;
	      }
	    
	  }

	double z;
	    if(drop_cutter(clus,x,ymax,r,safe_z,&z))
	      toolpath.push_back(point(x,ymax,z));
	    else
	      toolpath.push_back(point(x,ymax,safe_z));
	 
	forward_move=!forward_move;
      }
    else
      {
	for(double y=ymax; y>ymin; y-=dy)
	  {
	    double z;
	    if(drop_cutter(clus,x,y,r,safe_z,&z))
	      toolpath.push_back(point(x,y,z));
	    else
	      toolpath.push_back(point(x,y,safe_z));

	    if((++pc/ps*100.0)>perc+10)
	      {perc+=10;
		cerr<<"\rtracing: "<<perc<<"% rejected:"<<reject<<" accepted:"<<accept;
	      }
	    
	  }
	double z;
	if(drop_cutter(clus,x,ymin,r,safe_z,&z))
	  toolpath.push_back(point(x,ymin,z));
	else
	  toolpath.push_back(point(x,ymin,safe_z));
	forward_move=!forward_move;
      }
  
  return toolpath;
  
}


  
	    
    


// ###########################################################################
// ###########################################################################
// ###########################################################################
// ###########################################################################
// ###########################################################################
// ###########################################################################
// ###########################################################################
// ###########################################################################

bool intersect_triangle(const triang &tr, const matr &ori, double r,double *z)
{
  vector<uvector> tri;
  for(int i=0; i<3; i++)
    tri.push_back(uvector(4));
  
  for(int i=0; i<3; i++)
    {
      tri[i](0)=tr[i].x;
      tri[i](1)=tr[i].y;
      tri[i](2)=tr[i].z;
      tri[i](3)=1;
      
    }
  
  matr ior=special_inverse(ori);

  
  for(int i=0; i<3; i++)
    tri[i]=prod(ior,tri[i]);

   
   triang t;
  
   for(int i=0; i<3; i++)
     t.push_back(point(tri[i](0),tri[i](1),tri[i](2)));

   // cerr<<ori<<endl<<endl;
   
  
   return   intersect_triangle(t,/*ori(0,3)*/0,/*ori(1,3)*/0,r,z);
  

}

  
  

void drop_cylinder_cutter_2d::down_sort(double dx,double dy, cluster &parent)
 {
   if(parent.clus.size()!=0)
    {
      for(vector<cluster>::iterator p=parent.clus.begin();
	  p!=parent.clus.end();
	  ++p)
	down_sort(dx,dy,*p);
      return;
      
    }
   
      
  
  parent.clus.clear();
  
  const double overhit=0.01;
  
  double xmin=parent.xmin-overhit;
  double xmax=parent.xmax+overhit;

  double ymin=parent.ymin-overhit;
  double ymax=parent.ymax+overhit;
  
  double cx=(xmax-xmin)/dx;
  double cy=(ymax-ymin)/dy;
  
  
  double ps=(xmax-xmin)*(ymax-ymin)/cx/cy;
  double pc=0;
  int perc=0;

  int max_tris=0;
  int min_tris=10000;
  int nul_cluss=0;
  int av_tris=0;
  
  // first trace
  
  for(double x=xmin;x<=xmax;x+=cx)
    for(double y=ymin;y<=ymax;y+=cy)
      {
	
	cluster cl;
	cl.xmin=x;
	cl.xmax=x+cx;
	cl.ymin=y;
	cl.ymax=y+cy;

	for(triar::iterator tri=parent.tris.begin();
	    tri!=parent.tris.end();
	    ++tri)
	  { triang pnt;
	    // (1) cluster points in face
	    if(in_tri(*tri,x,y))
	      goto hit;
	    if(in_tri(*tri,x+cx,y))
	      goto hit;
	    if(in_tri(*tri,x+cx,y+cx))
	      goto hit;
	    if(in_tri(*tri,x,y+cx))
	      goto hit;
	    
	    // (2) face points in cluster
	    for(triang::const_iterator p=tri->begin();
		p!=tri->end();
		++p)
	      if(p->x>=x && p->x<=x+cx &&
		 p->y>=y && p->y<=y+cy)
		goto hit;
	    

	    // (3) cluster bounds cross face bounds;
	   
	    pnt.push_back(point(x,y,0));
	    pnt.push_back(point(x+cx,y,0));
	    pnt.push_back(point(x+cx,y+cx,0));
	    pnt.push_back(point(x,y+cx,0));
	    for(int i=0; i<3; i++)
	      {
		int ii=(i!=2)?i+1:0;
		for(int j=0;j<4;j++)
		  {
		    int jj=(j!=3)?j+1:0;
		    if(cross_test2d((*tri)[i],(*tri)[ii],
				     pnt[j],pnt[jj]))
		      goto hit;
		    
		  }
	      }
	    continue;
	      hit:
	    cl.tris.push_back(*tri);
	  }

	int cnt=cl.tris.size();
	if(cnt!=0)
	  {
	    if(cnt>max_tris)
	      max_tris=cnt;
	    if(cnt<min_tris)
	      min_tris=cnt;
	    av_tris+=cnt;
	    parent.clus.push_back(cl);
	  }
	else
	  ++nul_cluss;
	

	//if((++pc/ps*100.0)>perc+1)
	//cerr<<"\rclustering: "<<++perc<<"%";
	
      }

  av_tris/=parent.clus.size();

  cerr<<"min: "<<min_tris<<" max: "<<max_tris<<" avg: "<<av_tris<<endl;
  

  
}

  
  

       
       
